package es.cv.gui;

// Import the basic graphics classes.  
// The problem here is that we read the image with OpenCV into a Mat object.  
// But OpenCV for java doesn't have the method "imshow", so, we got to use  
// java for that (drawImage) that uses Image or BufferedImage.  
// So, how to go from one the other... Here is the way...  
import java.awt.*;
import java.awt.image.BufferedImage;
import java.io.File;
import java.util.ArrayList;

import javax.swing.*;

import org.apache.commons.math3.ml.clustering.Cluster;
import org.apache.commons.math3.ml.clustering.DBSCANClusterer;
import org.apache.commons.math3.ml.clustering.DoublePoint;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDMatch;
import org.opencv.core.MatOfKeyPoint;
import org.opencv.core.MatOfRect;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.features2d.DMatch;
import org.opencv.features2d.DescriptorExtractor;
import org.opencv.features2d.DescriptorMatcher;
import org.opencv.features2d.FeatureDetector;
import org.opencv.features2d.Features2d;
import org.opencv.features2d.KeyPoint;
import org.opencv.highgui.VideoCapture;
import org.opencv.imgproc.Imgproc;
import org.opencv.objdetect.CascadeClassifier;

import es.cv.core.CoreModule;

public class CentroideSURF_DistanciaMejoradoClustering extends JPanel implements Runnable{
	private static final long serialVersionUID = 1L;
	private BufferedImage image;

	// Create a constructor method
	public CentroideSURF_DistanciaMejoradoClustering() {
		super();
	}

	private BufferedImage getimage() {
		return image;
	}

	private void setimage(BufferedImage newimage) {
		image = newimage;
		return;
	}

	/**
	 * Converts/writes a Mat into a BufferedImage.
	 * 
	 * @param matrix
	 *            Mat of type CV_8UC3 or CV_8UC1
	 * @return BufferedImage of type TYPE_3BYTE_BGR or TYPE_BYTE_GRAY
	 */
	public static BufferedImage matToBufferedImage(Mat matrix) {
		int cols = matrix.cols();
		int rows = matrix.rows();
		int elemSize = (int) matrix.elemSize();
		byte[] data = new byte[cols * rows * elemSize];
		int type;
		matrix.get(0, 0, data);
		switch (matrix.channels()) {
		case 1:
			type = BufferedImage.TYPE_BYTE_GRAY;
			break;
		case 3:
			type = BufferedImage.TYPE_3BYTE_BGR;
			// bgr to rgb
			byte b;
			for (int i = 0; i < data.length; i = i + 3) {
				b = data[i];
				data[i] = data[i + 2];
				data[i + 2] = b;
			}
			break;
		default:
			return null;
		}
		BufferedImage image2 = new BufferedImage(cols, rows, type);
		image2.getRaster().setDataElements(0, 0, cols, rows, data);
		return image2;
	}

	public void paintComponent(Graphics g) {
		BufferedImage temp = getimage();
		g.drawImage(temp, 10, 10, temp.getWidth(), temp.getHeight(), this);
	}
	
	//private static String fileXML = "xml" + File.separator + "cascade.xml";
	//private static String workingDirectory = System.getProperty("user.dir");

	//private static String xml = workingDirectory + File.separator + fileXML;

	public static ArrayList<DoublePoint> surf(Mat src1, Mat src2, Mat dst_img_matches, int dbScanPerFrameRadius, int dbScanPerFrameMinPoints){
		Mat gray1 = new Mat();
		Mat gray2 = new Mat();
		Imgproc.cvtColor(src1, gray1, Imgproc.COLOR_BGR2GRAY);
		Imgproc.cvtColor(src2, gray2, Imgproc.COLOR_BGR2GRAY);
		MatOfKeyPoint keypoints1 = new MatOfKeyPoint();
		MatOfKeyPoint keypoints2 = new MatOfKeyPoint();

		// construct a SURF object.
		FeatureDetector featureDetector = FeatureDetector.create(FeatureDetector.SURF); 
		
		// detect function finds the keypoint in the images
		// Each keypoint is a special structure which has many attributes like its (x,y) coordinates,
		// size of the meaningful neighbourhood, angle which specifies its orientation, 
		// response that specifies strength of keypoints etc.
		featureDetector.detect(gray1, keypoints1);
		featureDetector.detect(gray2, keypoints2);
		if(keypoints1.empty() || keypoints2.empty()) return new ArrayList<DoublePoint>();
		//System.out.println("Detected "+keypoints.size()+" keypoints");
		Mat result1 = new Mat();
		Mat result2 = new Mat();
		// drawKeypoints function draws the small circles on the locations of keypoints. 
		Features2d.drawKeypoints(gray1, keypoints1, result1);
		Features2d.drawKeypoints(gray2, keypoints2, result2);
		
		//CoreModule.writeMatToFile(result1, "sift_carta.jpg");
		//CoreModule.writeMatToFile(result2, "sift_carta2.jpg");

		//CoreModule.writeMatToFile(result1, "carta_sift.jpg");
		//CoreModule.writeMatToFile(result2, "carta_escalada_sift.jpg");

		//Features2d.drawKeypoints(gray, keypoints1, result, new Scalar(new Random().nextInt(256),new Random().nextInt(256),new Random().nextInt(256)), Features2d.DRAW_RICH_KEYPOINTS);
		
		DescriptorExtractor descriptorExtractor = DescriptorExtractor.create(DescriptorExtractor.SURF);
		Mat descriptors1 = new Mat();
		Mat descriptors2 = new Mat();
				
		descriptorExtractor.compute(gray1, keypoints1, descriptors1);
		descriptorExtractor.compute(gray2, keypoints2, descriptors2);

		//Mat img_matches = new Mat();
	    DescriptorMatcher matcher = DescriptorMatcher.create(DescriptorMatcher.BRUTEFORCE);
		MatOfDMatch matches = new MatOfDMatch();
		

//		Mat hu = new Mat();
//		Moments mom = new Moments();
//		mom = Imgproc.moments(gray1, false);
//		double x = mom.get_m10() / mom.get_m00();
//		double y = mom.get_m01() / mom.get_m00();
//		System.out.println(x);
//		System.out.println(y);
		
		matcher.match(descriptors1, descriptors2, matches);
		DMatch[] dm = matches.toArray();
		
		//int ik1, ik2;
		int ik2;
		//KeyPoint[] akeypoints1 = keypoints1.toArray();
		KeyPoint[] akeypoints2 = keypoints2.toArray();
		ArrayList<DoublePoint> doublePointKeyPoints = new ArrayList<DoublePoint>();

//		int totalP = dm.length;
//		double sumX1 = 0.0, sumX2 = 0.0;
//		double sumY1 = 0.0, sumY2 = 0.0;
//		double meanX1, meanX2, meanY1, meanY2;
		for (int i = 0; i < dm.length; i++) {
			//ik1 = dm[i].queryIdx;
			ik2 = dm[i].trainIdx;
//			sumX1 += akeypoints1[ik1].pt.x;
//			sumY1 += akeypoints1[ik1].pt.y;			
//			sumX2 += akeypoints2[ik2].pt.x;
//			sumY2 += akeypoints2[ik2].pt.y;	
			doublePointKeyPoints.add(new DoublePoint(new double[]{akeypoints2[ik2].pt.x,akeypoints2[ik2].pt.y}));
		}
//		meanX1 = sumX1 / totalP;
//		meanY1 = sumY1 / totalP;
//		meanX2 = sumX2 / totalP;
//		meanY2 = sumY2 / totalP;

		//System.out.println(meanX1+","+meanY1);
		//System.out.println(meanX2+","+meanY2);
		

		//Core.circle(src1, new Point(meanX1, meanY1), 10, new Scalar(0, 0, 255),-1);
		//Core.circle(src2, new Point(meanX2, meanY2), 10, new Scalar(0, 0, 255), -1);
		
		DBSCANClusterer<DoublePoint> dbscan = new DBSCANClusterer<DoublePoint>(dbScanPerFrameRadius, dbScanPerFrameMinPoints);
		ArrayList<Cluster<DoublePoint>> generatedClusters = (ArrayList<Cluster<DoublePoint>>) dbscan.cluster(doublePointKeyPoints);
		//System.err.println("TAMAÑO DE CLUSTER: " + generatedClusters.size());
	
		double sumX2 = 0.0;
		double sumY2 = 0.0;
		double meanX2, meanY2;
		//Mat clusteredKeypointsResult = gray2.clone();
		ArrayList<DoublePoint> resultArrayListPoints = new ArrayList<DoublePoint>();
		if(generatedClusters.size() > 0)
		{
			for (int k = 0; k < generatedClusters.size(); k++) {
				ArrayList<DoublePoint> listOfDoublePoints = (ArrayList<DoublePoint>) generatedClusters.get(k).getPoints();
				for (int i = 0; i < listOfDoublePoints.size(); i++) {
					double[] current = listOfDoublePoints.get(i).getPoint();

					sumX2 += current[0];
					sumY2 += current[1];	
					
					//Core.circle(src2, new Point(current[0], current[1]), 2, new Scalar(255, 0, 0),-1);
				}
				meanX2 = sumX2 /  listOfDoublePoints.size();
				meanY2 = sumY2 /  listOfDoublePoints.size();
				DoublePoint centroid = new DoublePoint(new double[]{meanX2, meanY2});
				resultArrayListPoints.add(centroid);
				//Core.circle(src2, new Point(meanX2, meanY2), 10, new Scalar(0, 0, 255), -1);
			}

			//CoreModule.writeMatToFile(clusteredKeypointsResult, "pruebaDePuntos.jpg");
		}
		return resultArrayListPoints;
		
		//CoreModule.writeMatToFile(src1, "point_sift_carta_todos.jpg");
		//CoreModule.writeMatToFile(src2, "point_sift_carta2_todos.jpg");
//		
//		
//		
//		
//		ArrayList<DMatch> goodDMatches = new ArrayList<DMatch>();
//		double minGood = 0.6*0.48188045620918274;
//		for (int i = 0; i < dm.length; i++) {
//			if(dm[i].distance < minGood) goodDMatches.add(dm[i]);
//		}
//		System.out.println("De "+dm.length+", nos quedamos con "+goodDMatches.size()+" buenos.");
//		for (DMatch dMatch : goodDMatches) {
//			//System.out.println(dMatch.distance);
//		}
//		
//		int ik11, ik21;
//		KeyPoint[] akeypoints11 = keypoints1.toArray();
//		KeyPoint[] akeypoints21 = keypoints2.toArray();
//
//		int totalPP = goodDMatches.size();
//		double sumX11 = 0.0, sumX22 = 0.0;
//		double sumY11 = 0.0, sumY22 = 0.0;
//		double meanX11, meanX22, meanY11, meanY22;
//		for (int i = 0; i < goodDMatches.size(); i++) {
//			ik11 = goodDMatches.get(i).queryIdx;
//			ik21 = goodDMatches.get(i).trainIdx;
//			sumX11 += akeypoints11[ik11].pt.x;
//			sumY11 += akeypoints11[ik11].pt.y;			
//			sumX22 += akeypoints21[ik21].pt.x;
//			sumY22 += akeypoints21[ik21].pt.y;	
//		}
//		meanX11 = sumX11 / totalPP;
//		meanY11 = sumY11 / totalPP;
//		meanX22 = sumX22 / totalPP;
//		meanY22 = sumY22 / totalPP;

		//System.out.println(meanX11+","+meanY11);
		//System.out.println(meanX22+","+meanY22);
		

		//Core.circle(src1, new Point(meanX11, meanY11), 10, new Scalar(0, 255, 0),-1);
	//	Core.circle(src2, new Point(meanX22, meanY22), 10, new Scalar(0, 255, 0), -1);
		
		//CoreModule.writeMatToFile(src1, "point_sift_carta.jpg");
		//CoreModule.writeMatToFile(src2, "point_sift_carta2.jpg");
		
//		System.out.println(matches.rows());
//		System.out.println(matches.cols());
//		for (int i = 0; i < matches.rows(); i++) {
//			for (int j = 0; j < matches.cols(); j++) {
//				for (int j2 = 0; j2 < matches.get(i, j).length; j2++) {
//					 System.out.print(matches.get(i, j)[j2]+", ");
//				}
//				System.out.println("");
//			}
//		}
//		
//		Mat hu = new Mat();
//		Moments mom = new Moments();
//		mom = Imgproc.moments(matches, false);
//		double x = mom.get_m10() / mom.get_m00();
//		double y = mom.get_m01() / mom.get_m00();
//		System.out.println(x);
//		System.out.println(y);
		
		//MatOfDMatch newMatches = new MatOfDMatch();
		//newMatches.fromList(goodDMatches);
		
		//Mat dst_img_matches_buenos = new Mat();

		
		//Features2d.drawMatches(gray1, keypoints1, gray2, keypoints2, matches, dst_img_matches);

		//Features2d.drawMatches(gray1, keypoints1, gray2, keypoints2, newMatches, dst_img_matches_buenos);
		
		
		//CoreModule.writeMatToFile(dst_img_matches, "img_matches_todos.jpg");
		
		//CoreModule.writeMatToFile(dst_img_matches_buenos, "img_matches_buenos.jpg");
	
	}
	
	
	public static void main(String arg[]) {
		// Load the native library.
		System.loadLibrary(Core.NATIVE_LIBRARY_NAME);
		JFrame frame = new JFrame("SURF-Centroide");
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		frame.setSize(900, 600);
		CentroideSURF_DistanciaMejoradoClustering panel = new CentroideSURF_DistanciaMejoradoClustering();
		frame.setContentPane(panel);
		frame.setVisible(true);
		Mat webcam_image = new Mat();
		BufferedImage temp;
		VideoCapture capture = new VideoCapture(0);
		if (capture.isOpened()) {
			while (true) {
				capture.read(webcam_image);
				if (!webcam_image.empty()) {
					frame.setSize(webcam_image.width() + 40,
							webcam_image.height() + 60);
					
					// Detect face
					//CascadeClassifier faceDetector = new CascadeClassifier(xml);
					//MatOfRect faceDetections = new MatOfRect();
					//faceDetector.detectMultiScale(webcam_image, faceDetections);

					//for (Rect rect : faceDetections.toArray()) {
						//Core.rectangle(webcam_image, new Point(rect.x, rect.y), new Point(rect.x + rect.width, rect.y + rect.height), new Scalar(0,
							//	255, 0));
					//}
					
					Mat src1 = CoreModule.readMatFromImageFile("carta0.jpg");
					//Mat src2 = CoreModule.readMatFromImageFile("carta2.jpg");
					Mat dst = new Mat();
					surf(src1, webcam_image, dst, 30, 15 );
					
					temp = matToBufferedImage(webcam_image);
					panel.setimage(temp);
					panel.repaint();
				} else {
					System.out.println(" --(!) No captured frame -- Break!");
					break;
				}
			}
		}
		return;
	}

	public void run() {
		main(null);
	}
}